import numpy as np
from numpy import ndarray
from matplotlib import pyplot as plt


def bezier_interp(len_rank, lst_point: ndarray, lst_t: ndarray):
    """

    :param len_rank:
    :param lst_point: 输入点的坐标, [len, dim] 例如 [10, 2]
    :param lst_t:
    :return:
    """
    # 输入点个数, 曲线阶数
    n, nk = len_rank
    if nk > n - 1:
        raise RuntimeError('Bezier curve order must < n-1')
    tlen: int = lst_t.shape[0]
    ndim = lst_point.shape[1]  # point 的维度
    # {t参数长度, 插值剩余点, 坐标次序,}
    p_intp = np.zeros((tlen, n - nk, ndim), dtype=float)

    for dim in range(ndim):
        tmp = lst_point[:, dim].copy()  # 0阶插值
        for (it, t) in enumerate(lst_t):
            # 插值迭代，从 1 阶 到 nk 阶
            for k in range(1, nk + 1):
                tmp[:n - k] = (1 - t) * tmp[:n - k] + t * tmp[1:n - k + 1]
            p_intp[it, :, dim] = tmp[:n - nk]

    return p_intp


if __name__ == '__main__':
    lst = np.array((
        [0, 0], [1, 3], [2, 4], [3, 3], [4, 2], [5, 7]
    ), dtype=float)
    lst_t = np.linspace(0, 1, num=500, dtype=float)

    ret = bezier_interp((6, 5), lst, lst_t)
    # print(ret[..., 0])

    plt.plot(lst[:, 0], lst[:, 1], '-or',
             ret[:, 0, 0], ret[:, 0, 1], '-b')
    plt.autoscale(True)
    plt.show()
